#!/usr/bin/python
# -*- coding: UTF-8 -*-
from ctypes import *
import os
import time

udp_lib_name = "MiniBotControlLib.x64.so"
serial_lib_name = "MiniBotControlLib.LinuxSerial.x64.so"

dir_path = os.path.dirname(os.path.realpath(__file__)) + "/../"
folder_name = "01.DLL/x64/"

udp_lib_path = dir_path+folder_name+"/"+udp_lib_name
serial_lib_path = dir_path+folder_name+"/"+serial_lib_name


class MiniBotControlLib:
    def __init__(self, connect_mode):
        self.connet_mode = connect_mode
        if self.connet_mode == "udp":
            self.lib = cdll.LoadLibrary(udp_lib_path)
        elif self.connet_mode == "serial":
            self.lib = cdll.LoadLibrary(serial_lib_path)
        else:
            print("\033[31m MiniBotControlLib连接模式错误，请检查！\033[37m")
            exit(0)
   # 扫描在线机器人设备:默认58899 仿真器
   # 扫描在线机器人设备
    def ScanOnlineRobots(self, port=0):
        charPP = POINTER(c_char_p)
        ipPPvalue = c_char_p(5)
        ipPP = charPP(ipPPvalue)
        int_p = POINTER(c_int)
        countObject = c_int(4)
        count = int_p(countObject)
        num = 0
        if(port > 0):
            num = self.lib.ScanOnlineRobotsWithPort(c_int(port), ipPP, count)
        else:
            num = self.lib.ScanOnlineRobots(ipPP, count)
        robotsLs = []
        for i in range(num):
            robotsLs.append(str(ipPP[i]))
        return robotsLs


# 连接目标机器人设备:IP，端口，超时时间（毫秒）

    def Connect(self, ip, port, timeout=0):
        if (timeout < 0):
            r = self.lib.Connect(c_char_p(ip.encode('utf-8')), c_int(port))
        else:
            r = self.lib.ConnectWithTimeOut(
                c_char_p(ip.encode('utf-8')), c_int(port), c_int(timeout))
        if r > 0:
            pass
        else:
            print("\033[31m连接机器人失败\033[37m")
            exit(0)
        return r

   # 断开目标机器人设备
    def Disconnect(self):
        r = self.lib.Disconnect()
        return r

   # 扫描机器人底座连接的所有模块ID【返回Id 及 对应位置 列表】
    def ScanId(self, startId=20, endId=50):
        self.lib.ScanId.argtypes = [
            c_ubyte, c_ubyte, POINTER(c_ubyte), POINTER(c_ubyte)]
        self.lib.ScanId.restype = c_int
        id = c_ubyte(0)
        position = c_ubyte(0)
        count = 5
        while(True):
            num = self.lib.ScanId(startId, endId, id, position)
            count = count - 1
            time.sleep(1)
            if num > 0:
                break
            if count < 0:
                print("未检测到模块！请检查您是否连接")
                break
        id_p = pointer(id)
        position_p = pointer(position)
        idLs = []
        posLs = []
        for i in range(num):
            idLs.append(id_p[i])
        for i in range(num):
            posLs.append(position_p[i])
        # print("idLs", idLs)
        # print("posLs", posLs)
        # # 去除数组中重复的ID
        # import numpy as np
        # idLs = np.unique(idLs).tolist()
        # posLs = np.unique(posLs).tolist()
        # if len(idLs)!=len(posLs):
        #     print("\033[31m模块有重复ID，请检查！\033[37m")
        return idLs, posLs

    # 设置模块的ID，设置的ID应满足以下ID段，否则使用时可能会造成硬件损伤
    def SetMotorId(self, oldId, newId):
        count = 5
        while(True):
            r = self.lib.SetMotorId(c_ubyte(oldId), c_ubyte(newId))
            count = count - 1
            time.sleep(1)
            if r > 0:
                break
            if count < 0:
                print("未能设置模块ID，请检查！")
                break  
        return r

    # 设置模块的ID，设置的ID应满足以下ID段，否则使用时可能会造成硬件损伤
    def SetMotorITGType(self, id, itgType):
        count = 5
        while(True):
            r = self.lib.SetMotorITGType(c_ubyte(id), c_ubyte(itgType))
            count = count - 1
            time.sleep(1)
            if r > 0:
                break
            if count < 0:
                print("未能设置模块ITG类型，请检查！")
                break  
        return r

    # 设置模块的ID，设置的ID应满足以下ID段，否则使用时可能会造成硬件损伤
    def GetMotorITGType(self, id):
        r = self.lib.GetMotorITGType(c_ubyte(id))
        return r

   # 获取指定ID关节模块的位置角度值
    def GetMotorPosition(self, id):
        cid = c_ubyte(id)
        self.lib.GetMotorPosition.restype = c_int
        angle = self.lib.GetMotorPosition(cid)
        return angle

   # 获取指定ID关节模块的速度信息-【关节处于位置模式】
    def GetMotorTargetSpeed(self, id):
        cid = c_ubyte(id)
        self.lib.GetMotorTargetSpeed.restype = c_int
        speed = self.lib.GetMotorTargetSpeed(cid)
        return speed

   # 获取指定ID关节模块的速度信息-【关节模块处于速度模式】

    def GetMotorCurrentSpeed(self, id):
        cid = c_ubyte(id)
        self.lib.GetMotorCurrentSpeed .restype = c_int
        speed = self.lib.GetMotorCurrentSpeed(cid)
        return speed

   # 读取模块舵机参数
    def GetMotorParamter(self, id, address, size):
        cid = c_ubyte(id)
        c_addr = c_ubyte(address)
        c_size = c_ubyte(size)
        r = self.lib.GetMotorParamter(cid, c_addr, c_size)
        return r

   # 设置模块舵机参数
    def SetMotorParamter(self, id, address, size, val):
        cid = c_ubyte(id)
        c_addr = c_ubyte(address)
        c_size = c_ubyte(size)
        c_val = c_int(val)
        r = self.lib.SetMotorParamter(cid, c_addr, c_size, c_val)
        return r

   # 设置关节模块位置角度（位置模式）
    def SetMotorPosition(self, id, angle, wait_time=0):
        r = self.lib.SetMotorPosition(c_ubyte(id), c_int(angle))
        if wait_time != 0:
            time.sleep(wait_time)
        return r

   # 设置关节模块以指定速度运行到指定位置角度
    def SetPositionWithSpeed(self, id, angle, speed, wait_time=0):
        r = self.lib.SetMotorPositionWithSpeed(
            c_ubyte(id), c_int(angle), c_int(speed))
        if wait_time != 0:
            time.sleep(wait_time)
        return r

   # 同步设置多个关节模块的位置
    def SetMotorPositionSync(self, ids, angles, wait_time=0):
        size = len(ids)
        idLs = (c_ubyte * size)()
        posLs = (c_int * size)()
        for i in range(size):
            idLs[i] = (c_ubyte(ids[i]))
            posLs[i] = (c_int(angles[i]))
        r = self.lib.SetMotorPositionSync(idLs, posLs, c_int(size))
        if wait_time != 0:
            time.sleep(wait_time)
        return r

        # 以一定的速度同步设置多个关节模块的位置
    def SetMotorPositionSyncWithSpeeds(self, ids, angles, speeds, wait_time=0):
        size = len(ids)
        idLs = (c_ubyte * size)()
        posLs = (c_int * size)()
        speedLs = (c_int * size)()

        for i in range(size):
            idLs[i] = (c_ubyte(ids[i]))
            posLs[i] = (c_int(angles[i]))
            speedLs[i] = (c_int(speeds[i]))
        r = self.lib.SetMotorPositionSyncWithSpeeds(
            idLs, posLs, speedLs, c_int(size))
        if wait_time != 0:
            time.sleep(wait_time)
        return r

   # 设置指定ID的目标运行速度
    def SetMotorTargetSpeed(self, id, speed):
        r = self.lib.SetMotorTargetSpeed(c_ubyte(id), c_int(speed))
        return r

    # 同步设置多个关节模块的速度
    def SetMotorTargetSpeedSync(self, ids, speeds):
        size = len(ids)
        idLs = (c_ubyte * size)()
        speedLs = (c_int * size)()

        for i in range(size):
            idLs[i] = (c_ubyte(ids[i]))
            speedLs[i] = (c_int(speeds[i]))
        r = self.lib.SetMotorTargetSpeedSync(idLs, speedLs, c_int(size))
        return r

   # 设置模块是否锁定不可写id,isLock(0：锁关闭；1：锁开启)
    def SetMotorLocked(self, id, isLock):
        r = self.lib.SetMotorLocked(c_ubyte(id), bool(isLock))
        return r

   # 设置指定ID关节是否使能：id,isEnable(0：关；1：开启使能)
    def EnableMotor(self, id, isEnable):
        r = self.lib.EnableMotor(c_ubyte(id), bool(isEnable))
        return r

   # 设置关节模块模式：mode(0位置,1速度,2开环调速度模式)
    def SetMotorMode(self, id, mode):
        r = self.lib.SetMotorMode(c_ubyte(id), c_int(mode))
        return r

   # 设置当前连接的机器人设备的连接模式为STA模式，即连接到公网WIFI热点 注：不支持5G网络
    def SetSTAMode(self, ssid, pwd):
        r = self.lib.SetSTAMode(bytes(ssid.encode()), bytes(pwd.encode()))
        return r

   # 设置当前连接的机器人设备的连接模式为AP模式，即将当前设备设置为可连接的热点
    def SetAPMode(self, ssid, pwd):
        r = self.lib.SetAPMode(bytes(ssid.encode()), bytes(pwd.encode()))
        return r

   # 设置小移动平台灯带模式（如果存在）
    def SetLEDBel(self, val):
        self.lib.SetLEDBel.restype = c_int
        r = self.lib.SetLEDBel(c_int(val))
        return r

   # 读取外部设备数据(需连接传感器模块)
    def GetSensorData(self, id, ioPort, timeout=3000):
        r = self.lib.GetSensorData(
            c_ubyte(id), c_ubyte(ioPort), c_int(timeout))
        return r

   # 设置传感器数据
    def SetSensorData(self, id, ioPort, data):
        dataLen = len(data)
        data_array = (c_ubyte * dataLen)()
        for i in range(dataLen):
            data_array[i] = (c_ubyte(data[i]))
        r = self.lib.SetSensorData(c_ubyte(id), c_ubyte(
            ioPort), data_array, c_int(dataLen))
        return r

   # 从数据缓冲区中读取设备数据（例如：红外按钮）
    def ReadSensorData(self, id, ioPort, timeout=3000):
        r = self.lib.ReadSensorData(
            c_ubyte(id), c_ubyte(ioPort), c_int(timeout))
        return r

    # 设置离线运行姿态信息
    # 参考：https://blog.csdn.net/Yonggie/article/details/100584532
    # int SetRobotOfflineConfig(unsigned char* ids, int** angles, unsigned char
    # idCount, int* speeds, int* stayseconds, unsigned char count, unsigned char
    # runCount)
    def SetRobotOfflineConfig(self, ids, angles, speeds, stayseconds, runCount):
        int_Point = POINTER(c_int)
        idCount = len(ids)
        count = len(angles)
        idLs = (c_ubyte * idCount)()
        anglesRowType = (c_int * idCount)
        anglesRowType_p = POINTER(anglesRowType)
        anglesType = anglesRowType_p * count
        anglesLs = anglesType()
        # anglesLs=((c_int*idCount)*count)()
        speedLs = (c_int * count)()
        staysecondsLs = (c_int * count)()

        for i in range(idCount):
            idLs[i] = (c_ubyte(ids[i]))

        for i in range(count):
            speedLs[i] = (c_int(speeds[i]))
            staysecondsLs[i] = (c_int(stayseconds[i]))
            anglesRw = (c_int * idCount)()
            for d in range(idCount):
                anglesRw[d] = c_int(angles[i][d])
            anglesLs[i] = anglesRowType_p(anglesRw)
            # for d in range(idCount):
            #	anglesLs[i][d] = c_int(angles[i][d])
        #anglesLs_p = int_Point(anglesLs)
        r = self.lib.SetRobotOfflineConfig(idLs, anglesLs, c_ubyte(
            idCount), speedLs, staysecondsLs, c_ubyte(count), c_ubyte(runCount))
        return r

    # 设置是否启用离线运行模式
    def SetRobotOfflineMode(self, isEnable):
        r = self.lib.SetRobotOfflineMode(bool(isEnable))
        return r

   # 设置PWM(运行时间)
    def SetPWM(self, id, value):
        r = self.lib.SetPWM(c_ubyte(id), c_int(value))
        return r

   # 获取PWM(运行时间)
    def GetPWM(self, id):
        r = self.lib.GetPWM(c_ubyte(id))
        return r

   # 设置位置环P 比例系数
    def SetRingP(self, id, value):
        r = self.lib.SetPWM(c_ubyte(id), c_ubyte(value))
        return r

   # 设置位置环D 微分系数
    def SetRingD(self, id, value):
        r = self.lib.SetPWM(c_ubyte(id), c_ubyte(value))
        return r

   # 设置位置环I 积分系数
    def SetRingI(self, id, value):
        r = self.lib.SetPWM(c_ubyte(id), c_ubyte(value))
        return r

   # 设置卸载条件
    def SetProtectConditions(self, id, isVoltage, isSensor, isTemperature, isElectricity, isAngle, isOverload):
        r = self.lib.SetProtectConditions(c_ubyte(id), bool(isVoltage), bool(isSensor), bool(
            isTemperature), bool(isElectricity), bool(isAngle), bool(isOverload))
        return r

   # 设置LED报警条件
    def SetLedConditions(self, id, isVoltage, isSensor, isTemperature, isElectricity, isAngle, isOverload):
        r = self.lib.SetLedConditions(c_ubyte(id), bool(isVoltage), bool(isSensor), bool(
            isTemperature), bool(isElectricity), bool(isAngle), bool(isOverload))
        return r

    # 设置关节模块位置角度（位置模式）
    def SetIdsPositions(self, ids, angles):
        size = len(ids)
        for i in range(size):
            self.SetPosition(ids[i], angles[i])

   # 设置是否显示调试信息
    def SetIsDebug(self, isDebug):
        r = self.lib.SetIsDebug(bool(isDebug))
        return r

   # 配置里程计
    def ConfigOdometer(self, idLeft, idRight, whl_rad):
        r = self.lib.ConfigOdometer(
            c_ubyte(idLeft), c_ubyte(idRight), c_int(whl_rad))
        return r

   # 复位里程计数据
    def ResetOdometer(self):
        r = self.lib.ResetOdometer()
        return r

   # 配置里程计
    def EnableOdometer(self, isEnable):
        r = self.lib.EnableOdometer(bool(isEnable))
        return r

    def ReadOdometer(self):
        int_P = POINTER(c_int)
        odom_value = c_int(5)
        odomPP = int_P(odom_value)
        flag = self.lib.ReadOdometer(odomPP)
        odmLs = []
        if(flag > 0):
            for i in range(5):
                odmLs.append(odomPP[i])
            return odmLs
        else:
            return odmLs
